﻿using System;
using System.Collections.Generic;
using System.Text;
using System.Windows.Forms;

namespace SCI_actuator
{
    class GroupCommand
    {
        private menu_control frmain;

        public GroupCommand(menu_control fr)
        {
            this.frmain = fr;
        }

        private void Delay_ms(int mm)
        {
            DateTime current = DateTime.Now;

            while (current.AddMilliseconds(mm) > DateTime.Now)
            {
                Application.DoEvents();
            }
            return;
        }

        //给1个主舵机发送Hermit曲线的位置和时间信息  
        public void AddPoint_Curve1(string number, string ph1, string pl1, string dh1, string dl1)  //number为舵机编号
        {
            string pos_hi1 = ph1;
            string pos_low1 = pl1;
            string del_hi1 = dh1;
            string del_low1 = dl1;
            string num = number;

            string sum_str = null;
            string curve = null;
            byte[] bRx;
            byte sum = 0;

            bRx = this.frmain.HexStringToByteArray(num + "0C" + "02"
               + constant.REG_CURVE_POSITION_HI + pos_hi1 + constant.REG_CURVE_POSITION_LO + pos_low1
               + constant.REG_CURVE_DELTA_HI + del_hi1 + constant.REG_CURVE_DELTA_LO + del_low1 + constant.REG_CURVE_APPEND + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + num + "0C" + "02"
               + constant.REG_CURVE_POSITION_HI + pos_hi1 + constant.REG_CURVE_POSITION_LO + pos_low1
               + constant.REG_CURVE_DELTA_HI + del_hi1 + constant.REG_CURVE_DELTA_LO + del_low1 + constant.REG_CURVE_APPEND + "01" + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);

            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }

            Delay_ms(10);
        }

        //给3个主舵机发送Hermit曲线的位置和时间信息
        public void AddPoint_Curve(string ph1, string pl1, string dh1, string dl1, string ph2, string pl2, string dh2, string dl2, string ph3, string pl3, string dh3, string dl3)
        {
            string pos_hi1 = ph1;
            string pos_low1 = pl1;
            string del_hi1 = dh1;
            string del_low1 = dl1;
            string pos_hi2 = ph2;
            string pos_low2 = pl2;
            string del_hi2 = dh2;
            string del_low2 = dl2;
            string pos_hi3 = ph3;
            string pos_low3 = pl3;
            string del_hi3 = dh3;
            string del_low3 = dl3;
            string sum_str = null;
            string curve = null;
            byte[] bRx;
            byte sum = 0;

            bRx = this.frmain.HexStringToByteArray("FE" + "25"
               + "02" + "0A" + constant.REG_CURVE_POSITION_HI + pos_hi1 + constant.REG_CURVE_POSITION_LO + pos_low1
               + constant.REG_CURVE_DELTA_HI + del_hi1 + constant.REG_CURVE_DELTA_LO + del_low1 + constant.REG_CURVE_APPEND + "01"
               + "03" + "0A" + constant.REG_CURVE_POSITION_HI + pos_hi2 + constant.REG_CURVE_POSITION_LO + pos_low2
               + constant.REG_CURVE_DELTA_HI + del_hi2 + constant.REG_CURVE_DELTA_LO + del_low2 + constant.REG_CURVE_APPEND + "01"
               + "04" + "0A" + constant.REG_CURVE_POSITION_HI + pos_hi3 + constant.REG_CURVE_POSITION_LO + pos_low3
               + constant.REG_CURVE_DELTA_HI + del_hi3 + constant.REG_CURVE_DELTA_LO + del_low3 + constant.REG_CURVE_APPEND + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + "FE" + "25"
               + "02" + "0A" + constant.REG_CURVE_POSITION_HI + pos_hi1 + constant.REG_CURVE_POSITION_LO + pos_low1
               + constant.REG_CURVE_DELTA_HI + del_hi1 + constant.REG_CURVE_DELTA_LO + del_low1 + constant.REG_CURVE_APPEND + "01"
               + "03" + "0A" + constant.REG_CURVE_POSITION_HI + pos_hi2 + constant.REG_CURVE_POSITION_LO + pos_low2
               + constant.REG_CURVE_DELTA_HI + del_hi2 + constant.REG_CURVE_DELTA_LO + del_low2 + constant.REG_CURVE_APPEND + "01"
               + "04" + "0A" + constant.REG_CURVE_POSITION_HI + pos_hi3 + constant.REG_CURVE_POSITION_LO + pos_low3
               + constant.REG_CURVE_DELTA_HI + del_hi3 + constant.REG_CURVE_DELTA_LO + del_low3 + constant.REG_CURVE_APPEND + "01" + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);

            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }

            Delay_ms(10);
        }

        //给4个舵机发送Hermit曲线的位置和时间信息
        public void AddPoint_Curve4(string ph1, string pl1, string dh1, string dl1, string ph2, string pl2, string dh2, string dl2, string ph3, string pl3, string dh3, string dl3, string ph4, string pl4, string dh4, string dl4)
        {
            string pos_hi1 = ph1;
            string pos_low1 = pl1;
            string del_hi1 = dh1;
            string del_low1 = dl1;
            string pos_hi2 = ph2;
            string pos_low2 = pl2;
            string del_hi2 = dh2;
            string del_low2 = dl2;
            string pos_hi3 = ph3;
            string pos_low3 = pl3;
            string del_hi3 = dh3;
            string del_low3 = dl3;
            string pos_hi4 = ph4;
            string pos_low4 = pl4;
            string del_hi4 = dh4;
            string del_low4 = dl4;
            string sum_str = null;
            string curve = null;
            byte[] bRx;
            byte sum = 0;

            bRx = this.frmain.HexStringToByteArray("FE" + "31"
               + "01" + "0A" + constant.REG_CURVE_POSITION_HI + pos_hi1 + constant.REG_CURVE_POSITION_LO + pos_low1
               + constant.REG_CURVE_DELTA_HI + del_hi1 + constant.REG_CURVE_DELTA_LO + del_low1 + constant.REG_CURVE_APPEND + "01"
               + "02" + "0A" + constant.REG_CURVE_POSITION_HI + pos_hi2 + constant.REG_CURVE_POSITION_LO + pos_low2
               + constant.REG_CURVE_DELTA_HI + del_hi2 + constant.REG_CURVE_DELTA_LO + del_low2 + constant.REG_CURVE_APPEND + "01"
               + "03" + "0A" + constant.REG_CURVE_POSITION_HI + pos_hi3 + constant.REG_CURVE_POSITION_LO + pos_low3
               + constant.REG_CURVE_DELTA_HI + del_hi3 + constant.REG_CURVE_DELTA_LO + del_low3 + constant.REG_CURVE_APPEND + "01"
               + "04" + "0A" + constant.REG_CURVE_POSITION_HI + pos_hi4 + constant.REG_CURVE_POSITION_LO + pos_low4
               + constant.REG_CURVE_DELTA_HI + del_hi4 + constant.REG_CURVE_DELTA_LO + del_low4 + constant.REG_CURVE_APPEND + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + "FE" + "31"
               + "01" + "0A" + constant.REG_CURVE_POSITION_HI + pos_hi1 + constant.REG_CURVE_POSITION_LO + pos_low1
               + constant.REG_CURVE_DELTA_HI + del_hi1 + constant.REG_CURVE_DELTA_LO + del_low1 + constant.REG_CURVE_APPEND + "01"
               + "02" + "0A" + constant.REG_CURVE_POSITION_HI + pos_hi2 + constant.REG_CURVE_POSITION_LO + pos_low2
               + constant.REG_CURVE_DELTA_HI + del_hi2 + constant.REG_CURVE_DELTA_LO + del_low2 + constant.REG_CURVE_APPEND + "01"
               + "03" + "0A" + constant.REG_CURVE_POSITION_HI + pos_hi3 + constant.REG_CURVE_POSITION_LO + pos_low3
               + constant.REG_CURVE_DELTA_HI + del_hi3 + constant.REG_CURVE_DELTA_LO + del_low3 + constant.REG_CURVE_APPEND + "01"
               + "04" + "0A" + constant.REG_CURVE_POSITION_HI + pos_hi4 + constant.REG_CURVE_POSITION_LO + pos_low4
               + constant.REG_CURVE_DELTA_HI + del_hi4 + constant.REG_CURVE_DELTA_LO + del_low4 + constant.REG_CURVE_APPEND + "01" + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);

            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }

            Delay_ms(10);
        }

        //给1个主舵机发送连续曲线的位置和速度信息
        public void AddPoint_T1(string number, string ph1, string pl1, string dh1, string dl1)  //number为舵机编号
        {
            string pos_hi1 = ph1;
            string pos_low1 = pl1;
            string del_hi1 = dh1;
            string del_low1 = dl1;
            string num = number;
      
            string sum_str = null;
            string curve = null;
            byte[] bRx;
            byte sum = 0;

            bRx = this.frmain.HexStringToByteArray(num + "0C" + "02"
               + constant.REG_T_POSITION_HI + pos_hi1 + constant.REG_T_POSITION_LO + pos_low1 
               + constant.REG_T_VELOCITY_HI + del_hi1 + constant.REG_T_VELOCITY_LO + del_low1 + constant.REG_CURVE_APPEND + "02"
              );
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + num + "0C" + "02"
               + constant.REG_T_POSITION_HI + pos_hi1 + constant.REG_T_POSITION_LO + pos_low1 
               + constant.REG_T_VELOCITY_HI + del_hi1 + constant.REG_T_VELOCITY_LO + del_low1 + constant.REG_CURVE_APPEND + "02" + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);

            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }

            Delay_ms(10);
        }

        //给3个主舵机发送连续曲线的位置和速度信息
        public void AddPoint_T(string ph1, string pl1, string dh1, string dl1, string ph2, string pl2, string dh2, string dl2, string ph3, string pl3, string dh3, string dl3)
        {
            string pos_hi1 = ph1;
            string pos_low1 = pl1;
            string del_hi1 = dh1;
            string del_low1 = dl1;
            string pos_hi2 = ph2;
            string pos_low2 = pl2;
            string del_hi2 = dh2;
            string del_low2 = dl2;
            string pos_hi3 = ph3;
            string pos_low3 = pl3;
            string del_hi3 = dh3;
            string del_low3 = dl3;
            string sum_str = null;
            string curve = null;
            byte[] bRx;
            byte sum = 0;

            bRx = this.frmain.HexStringToByteArray("FE" + "25"
               + "02" + "0A" + constant.REG_T_POSITION_HI + pos_hi1 + constant.REG_T_POSITION_LO + pos_low1 + constant.REG_T_VELOCITY_HI + del_hi1 + constant.REG_T_VELOCITY_LO + del_low1 + constant.REG_CURVE_APPEND + "02"
               + "03" + "0A" + constant.REG_T_POSITION_HI + pos_hi2 + constant.REG_T_POSITION_LO + pos_low2 + constant.REG_T_VELOCITY_HI + del_hi2 + constant.REG_T_VELOCITY_LO + del_low2 + constant.REG_CURVE_APPEND + "02"
               + "04" + "0A" + constant.REG_T_POSITION_HI + pos_hi3 + constant.REG_T_POSITION_LO + pos_low3 + constant.REG_T_VELOCITY_HI + del_hi3 + constant.REG_T_VELOCITY_LO + del_low3 + constant.REG_CURVE_APPEND + "02");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + "FE" + "25"
               + "02" + "0A" + constant.REG_T_POSITION_HI + pos_hi1 + constant.REG_T_POSITION_LO + pos_low1 + constant.REG_T_VELOCITY_HI + del_hi1 + constant.REG_T_VELOCITY_LO + del_low1 + constant.REG_CURVE_APPEND + "02"
               + "03" + "0A" + constant.REG_T_POSITION_HI + pos_hi2 + constant.REG_T_POSITION_LO + pos_low2 + constant.REG_T_VELOCITY_HI + del_hi2 + constant.REG_T_VELOCITY_LO + del_low2 + constant.REG_CURVE_APPEND + "02"
               + "04" + "0A" + constant.REG_T_POSITION_HI + pos_hi3 + constant.REG_T_POSITION_LO + pos_low3 + constant.REG_T_VELOCITY_HI + del_hi3 + constant.REG_T_VELOCITY_LO + del_low3 + constant.REG_CURVE_APPEND + "02"
               + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);

            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }

            Delay_ms(10);
        }

        //给2个舵机发送连续曲线的位置和速度信息，这两个舵机可设置
        public void AddPoint_T2(string n1, string n2, string ph1, string pl1, string dh1, string dl1, string ph2, string pl2, string dh2, string dl2)
        {
            string num1 = n1;
            string num2 = n2;
            string pos_hi1 = ph1;
            string pos_low1 = pl1;
            string del_hi1 = dh1;
            string del_low1 = dl1;
            string pos_hi2 = ph2;
            string pos_low2 = pl2;
            string del_hi2 = dh2;
            string del_low2 = dl2;

            string sum_str = null;
            string curve = null;
            byte[] bRx;
            byte sum = 0;

            bRx = this.frmain.HexStringToByteArray("FE" + "19"
               + n1 + "0A" + constant.REG_T_POSITION_HI + pos_hi1 + constant.REG_T_POSITION_LO + pos_low1 + constant.REG_T_VELOCITY_HI + del_hi1 + constant.REG_T_VELOCITY_LO + del_low1 + constant.REG_CURVE_APPEND + "02"
               + n2 + "0A" + constant.REG_T_POSITION_HI + pos_hi2 + constant.REG_T_POSITION_LO + pos_low2 + constant.REG_T_VELOCITY_HI + del_hi2 + constant.REG_T_VELOCITY_LO + del_low2 + constant.REG_CURVE_APPEND + "02");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + "FE" + "19"
               + n1 + "0A" + constant.REG_T_POSITION_HI + pos_hi1 + constant.REG_T_POSITION_LO + pos_low1 + constant.REG_T_VELOCITY_HI + del_hi1 + constant.REG_T_VELOCITY_LO + del_low1 + constant.REG_CURVE_APPEND + "02"
               + n2 + "0A" + constant.REG_T_POSITION_HI + pos_hi2 + constant.REG_T_POSITION_LO + pos_low2 + constant.REG_T_VELOCITY_HI + del_hi2 + constant.REG_T_VELOCITY_LO + del_low2 + constant.REG_CURVE_APPEND + "02"
               + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);

            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }

            Delay_ms(10);
        }

        //给4个舵机发送连续曲线的位置和速度信息
        public void AddPoint_T4(string ph1, string pl1, string dh1, string dl1, string ph2, string pl2, string dh2, string dl2, string ph3, string pl3, string dh3, string dl3, string ph4, string pl4, string dh4, string dl4)
        {
            string pos_hi1 = ph1;
            string pos_low1 = pl1;
            string del_hi1 = dh1;
            string del_low1 = dl1;
            string pos_hi2 = ph2;
            string pos_low2 = pl2;
            string del_hi2 = dh2;
            string del_low2 = dl2;
            string pos_hi3 = ph3;
            string pos_low3 = pl3;
            string del_hi3 = dh3;
            string del_low3 = dl3;
            string pos_hi4 = ph4;
            string pos_low4 = pl4;
            string del_hi4 = dh4;
            string del_low4 = dl4;
            string sum_str = null;
            string curve = null;
            byte[] bRx;
            byte sum = 0;

            bRx = this.frmain.HexStringToByteArray("FE" + "31"
               + "01" + "0A" + constant.REG_T_POSITION_HI + pos_hi1 + constant.REG_T_POSITION_LO + pos_low1 + constant.REG_T_VELOCITY_HI + del_hi1 + constant.REG_T_VELOCITY_LO + del_low1 + constant.REG_CURVE_APPEND + "02"
               + "02" + "0A" + constant.REG_T_POSITION_HI + pos_hi2 + constant.REG_T_POSITION_LO + pos_low2 + constant.REG_T_VELOCITY_HI + del_hi2 + constant.REG_T_VELOCITY_LO + del_low2 + constant.REG_CURVE_APPEND + "02"
               + "03" + "0A" + constant.REG_T_POSITION_HI + pos_hi3 + constant.REG_T_POSITION_LO + pos_low3 + constant.REG_T_VELOCITY_HI + del_hi3 + constant.REG_T_VELOCITY_LO + del_low3 + constant.REG_CURVE_APPEND + "02"
               + "04" + "0A" + constant.REG_T_POSITION_HI + pos_hi4 + constant.REG_T_POSITION_LO + pos_low4 + constant.REG_T_VELOCITY_HI + del_hi4 + constant.REG_T_VELOCITY_LO + del_low4 + constant.REG_CURVE_APPEND + "02");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + "FE" + "31"
               + "01" + "0A" + constant.REG_T_POSITION_HI + pos_hi1 + constant.REG_T_POSITION_LO + pos_low1 + constant.REG_T_VELOCITY_HI + del_hi1 + constant.REG_T_VELOCITY_LO + del_low1 + constant.REG_CURVE_APPEND + "02"
               + "02" + "0A" + constant.REG_T_POSITION_HI + pos_hi2 + constant.REG_T_POSITION_LO + pos_low2 + constant.REG_T_VELOCITY_HI + del_hi2 + constant.REG_T_VELOCITY_LO + del_low2 + constant.REG_CURVE_APPEND + "02"
               + "03" + "0A" + constant.REG_T_POSITION_HI + pos_hi3 + constant.REG_T_POSITION_LO + pos_low3 + constant.REG_T_VELOCITY_HI + del_hi3 + constant.REG_T_VELOCITY_LO + del_low3 + constant.REG_CURVE_APPEND + "02"
               + "04" + "0A" + constant.REG_T_POSITION_HI + pos_hi4 + constant.REG_T_POSITION_LO + pos_low4 + constant.REG_T_VELOCITY_HI + del_hi4 + constant.REG_T_VELOCITY_LO + del_low4 + constant.REG_CURVE_APPEND + "02"
               + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);

            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }

            Delay_ms(10);
        }

        //以连续曲线开启一个舵机转动到指定位置   number为舵机编号
        public void Start_T1(string number, string ph1, string pl1, string dh1, string dl1)
        {
            string num = number;
            string pos_hi1 = ph1;
            string pos_low1 = pl1;
            string del_hi1 = dh1;
            string del_low1 = dl1;
            
            string sum_str = null;
            string curve = null;
            byte[] bRx;
            byte sum = 0;

            bRx = this.frmain.HexStringToByteArray(num + "04" + "02" + constant.REG_ACTION + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);

            curve = "FF" + "FF" + num + "04" + "02" + constant.REG_ACTION + "01" + sum_str;
            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }
            this.frmain.servosend(curve);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }

            Delay_ms(100);

            bRx = this.frmain.HexStringToByteArray(num + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi1
            + constant.REG_T_POSITION_LO + pos_low1 + constant.REG_T_VELOCITY_HI + del_hi1
            + constant.REG_T_VELOCITY_LO + del_low1 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);

            curve = "FF" + "FF" + num + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi1
            + constant.REG_T_POSITION_LO + pos_low1 + constant.REG_T_VELOCITY_HI + del_hi1
            + constant.REG_T_VELOCITY_LO + del_low1 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01" + sum_str;
            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }
            this.frmain.servosend(curve);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }

            Delay_ms(100);

            bRx = this.frmain.HexStringToByteArray(num + "04" + "02" + constant.REG_ACTION + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);

            curve = "FF" + "FF" + num + "04" + "02" + constant.REG_ACTION + "01" + sum_str;
            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }
            this.frmain.servosend(curve);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
        }
      
        //以连续曲线开启舵机转动到指定位置
        public void Start_T(string ph1, string pl1, string dh1, string dl1, string ph2, string pl2, string dh2, string dl2, string ph3, string pl3, string dh3, string dl3, string ph4, string pl4, string dh4, string dl4)
        {
            string pos_hi1 = ph1;
            string pos_low1 = pl1;
            string del_hi1 = dh1;
            string del_low1 = dl1;
            string pos_hi2 = ph2;
            string pos_low2 = pl2;
            string del_hi2 = dh2;
            string del_low2 = dl2;
            string pos_hi3 = ph3;
            string pos_low3 = pl3;
            string del_hi3 = dh3;
            string del_low3 = dl3;
            string pos_hi4 = ph4;
            string pos_low4 = pl4;
            string del_hi4 = dh4;
            string del_low4 = dl4;
            string sum_str = null;
            string curve = null;
            byte[] bRx;
            byte sum = 0;
            #region 01
            bRx = this.frmain.HexStringToByteArray("01" + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi1
            + constant.REG_T_POSITION_LO + pos_low1 + constant.REG_T_VELOCITY_HI + del_hi1
            + constant.REG_T_VELOCITY_LO + del_low1 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);

            curve = "FF" + "FF" + "01" + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi1
            + constant.REG_T_POSITION_LO + pos_low1 + constant.REG_T_VELOCITY_HI + del_hi1
            + constant.REG_T_VELOCITY_LO + del_low1 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01" + sum_str;
            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }
            this.frmain.servosend(curve);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            Delay_ms(100);
            bRx = this.frmain.HexStringToByteArray("01" + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi1
            + constant.REG_T_POSITION_LO + pos_low1 + constant.REG_T_VELOCITY_HI + del_hi1
            + constant.REG_T_VELOCITY_LO + del_low1 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);

            curve = "FF" + "FF" + "01" + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi1
            + constant.REG_T_POSITION_LO + pos_low1 + constant.REG_T_VELOCITY_HI + del_hi1
            + constant.REG_T_VELOCITY_LO + del_low1 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01" + sum_str;
            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }
            this.frmain.servosend(curve);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            Delay_ms(100);
            bRx = this.frmain.HexStringToByteArray("01" + "04" + "02" + constant.REG_ACTION + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);

            curve = "FF" + "FF" + "01" + "04" + "02" + constant.REG_ACTION + "01" + sum_str;
            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }
            this.frmain.servosend(curve);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            #endregion
            Delay_ms(100);
            #region 02
            bRx = this.frmain.HexStringToByteArray("02" + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi2
           + constant.REG_T_POSITION_LO + pos_low2 + constant.REG_T_VELOCITY_HI + del_hi2
           + constant.REG_T_VELOCITY_LO + del_low2 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);

            curve = "FF" + "FF" + "02" + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi2
           + constant.REG_T_POSITION_LO + pos_low2 + constant.REG_T_VELOCITY_HI + del_hi2
           + constant.REG_T_VELOCITY_LO + del_low2 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01" + sum_str;
            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }
            this.frmain.servosend(curve);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            
            Delay_ms(100);
            bRx = this.frmain.HexStringToByteArray("02" + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi2
           + constant.REG_T_POSITION_LO + pos_low2 + constant.REG_T_VELOCITY_HI + del_hi2
           + constant.REG_T_VELOCITY_LO + del_low2 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);

            curve = "FF" + "FF" + "02" + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi2
           + constant.REG_T_POSITION_LO + pos_low2 + constant.REG_T_VELOCITY_HI + del_hi2
           + constant.REG_T_VELOCITY_LO + del_low2 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01" + sum_str;
            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }
            this.frmain.servosend(curve);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            Delay_ms(100);
            bRx = this.frmain.HexStringToByteArray("02" + "04" + "02" + constant.REG_ACTION + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);

            curve = "FF" + "FF" + "02" + "04" + "02" + constant.REG_ACTION + "01" + sum_str;
            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }
            this.frmain.servosend(curve);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            #endregion
            Delay_ms(100);
            #region 03
            bRx = this.frmain.HexStringToByteArray("03" + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi3
           + constant.REG_T_POSITION_LO + pos_low3 + constant.REG_T_VELOCITY_HI + del_hi3
           + constant.REG_T_VELOCITY_LO + del_low3 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);

            curve = "FF" + "FF" + "03" + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi3
           + constant.REG_T_POSITION_LO + pos_low3 + constant.REG_T_VELOCITY_HI + del_hi3
           + constant.REG_T_VELOCITY_LO + del_low3 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01" + sum_str;
            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }
            this.frmain.servosend(curve);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            Delay_ms(100);
            bRx = this.frmain.HexStringToByteArray("03" + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi3
           + constant.REG_T_POSITION_LO + pos_low3 + constant.REG_T_VELOCITY_HI + del_hi3
           + constant.REG_T_VELOCITY_LO + del_low3 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);

            curve = "FF" + "FF" + "03" + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi3
           + constant.REG_T_POSITION_LO + pos_low3 + constant.REG_T_VELOCITY_HI + del_hi3
           + constant.REG_T_VELOCITY_LO + del_low3 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01" + sum_str;
            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }
            this.frmain.servosend(curve);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            Delay_ms(100);
            bRx = this.frmain.HexStringToByteArray("03" + "04" + "02" + constant.REG_ACTION + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);

            curve = "FF" + "FF" + "03" + "04" + "02" + constant.REG_ACTION + "01" + sum_str;
            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }
            this.frmain.servosend(curve);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            #endregion
            Delay_ms(100);
            #region 04
           bRx = this.frmain.HexStringToByteArray("04" + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi4
           + constant.REG_T_POSITION_LO + pos_low4 + constant.REG_T_VELOCITY_HI + del_hi4
           + constant.REG_T_VELOCITY_LO + del_low4 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);

           curve = "FF" + "FF" + "04" + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi4
           + constant.REG_T_POSITION_LO + pos_low4 + constant.REG_T_VELOCITY_HI + del_hi4
           + constant.REG_T_VELOCITY_LO + del_low4 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01" + sum_str;
            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }
            this.frmain.servosend(curve);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            Delay_ms(100);
            bRx = this.frmain.HexStringToByteArray("04" + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi4
            + constant.REG_T_POSITION_LO + pos_low4 + constant.REG_T_VELOCITY_HI + del_hi4
            + constant.REG_T_VELOCITY_LO + del_low4 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);

            curve = "FF" + "FF" + "04" + "0E" + "02" + constant.REG_T_POSITION_HI + pos_hi4
            + constant.REG_T_POSITION_LO + pos_low4 + constant.REG_T_VELOCITY_HI + del_hi4
            + constant.REG_T_VELOCITY_LO + del_low4 + constant.REG_CURVE_APPEND + "02" + constant.REG_MODEL + "01" + sum_str;
            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }
            this.frmain.servosend(curve);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            Delay_ms(100);
            bRx = this.frmain.HexStringToByteArray("04" + "04" + "02" + constant.REG_ACTION + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);

            curve = "FF" + "FF" + "04" + "04" + "02" + constant.REG_ACTION + "01" + sum_str;
            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }
            this.frmain.servosend(curve);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            #endregion
            /*bRx = this.frmain.HexStringToByteArray("FE" + "31"
               + "01" + "0A" + constant.REG_T_POSITION_HI + pos_hi1 + constant.REG_T_POSITION_LO + pos_low1
               + constant.REG_T_VELOCITY_HI + del_hi1 + constant.REG_T_VELOCITY_LO + del_low1 + constant.REG_CURVE_APPEND + "02"
               + "02" + "0A" + constant.REG_T_POSITION_HI + pos_hi2 + constant.REG_T_POSITION_LO + pos_low2
               + constant.REG_T_VELOCITY_HI + del_hi2 + constant.REG_T_VELOCITY_LO + del_low2 + constant.REG_CURVE_APPEND + "02"
               + "03" + "0A" + constant.REG_T_POSITION_HI + pos_hi3 + constant.REG_T_POSITION_LO + pos_low3
               + constant.REG_T_VELOCITY_HI + del_hi3 + constant.REG_T_VELOCITY_LO + del_low3 + constant.REG_CURVE_APPEND + "02"
               + "04" + "0A" + constant.REG_T_POSITION_HI + pos_hi4 + constant.REG_T_POSITION_LO + pos_low4
               + constant.REG_T_VELOCITY_HI + del_hi4 + constant.REG_T_VELOCITY_LO + del_low4 + constant.REG_CURVE_APPEND + "02");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + "FE" + "31"
               + "01" + "0A" + constant.REG_T_POSITION_HI + pos_hi1 + constant.REG_T_POSITION_LO + pos_low1
               + constant.REG_T_VELOCITY_HI + del_hi1 + constant.REG_T_VELOCITY_LO + del_low1 + constant.REG_CURVE_APPEND + "02"
               + "02" + "0A" + constant.REG_T_POSITION_HI + pos_hi2 + constant.REG_T_POSITION_LO + pos_low2
               + constant.REG_T_VELOCITY_HI + del_hi2 + constant.REG_T_VELOCITY_LO + del_low2 + constant.REG_CURVE_APPEND + "02"
               + "03" + "0A" + constant.REG_T_POSITION_HI + pos_hi3 + constant.REG_T_POSITION_LO + pos_low3
               + constant.REG_T_VELOCITY_HI + del_hi3 + constant.REG_T_VELOCITY_LO + del_low3 + constant.REG_CURVE_APPEND + "02"
               + "04" + "0A" + constant.REG_T_POSITION_HI + pos_hi4 + constant.REG_T_POSITION_LO + pos_low4
               + constant.REG_T_VELOCITY_HI + del_hi4 + constant.REG_T_VELOCITY_LO + del_low4 + constant.REG_CURVE_APPEND + "02"
               + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);

            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }

            Delay_ms(100);
 
            bRx = this.frmain.HexStringToByteArray("FE" + "19"
              + "01" + "04" + constant.REG_ACTION + "01" + constant.REG_MODEL + "01"
              + "02" + "04" + constant.REG_ACTION + "01" + constant.REG_MODEL + "01"
              + "03" + "04" + constant.REG_ACTION + "01" + constant.REG_MODEL + "01"
              + "04" + "04" + constant.REG_ACTION + "01" + constant.REG_MODEL + "01");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + "FE" + "19"
              + "01" + "04" + constant.REG_ACTION + "01" + constant.REG_MODEL + "01"
              + "02" + "04" + constant.REG_ACTION + "01" + constant.REG_MODEL + "01"
              + "03" + "04" + constant.REG_ACTION + "01" + constant.REG_MODEL + "01"
              + "04" + "04" + constant.REG_ACTION + "01" + constant.REG_MODEL + "01"
              + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);

            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            */
        }
        
        //判断其中一个舵机Hermit缓存区剩余点数
        public void BufferLeft_Curve(string number)   //number为舵机编号
        {
            string sum_str = null;
            string curve = null;
            byte sum = 0;

            byte[] bRx = this.frmain.HexStringToByteArray(number + "03" + "01" + constant.REG_CURVE_BUFFER);
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + number + "03" + "01" + constant.REG_CURVE_BUFFER + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);


            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            Delay_ms(10);
        }

        //判断其中一个舵机连续缓存区剩余点数
        public void BufferLeft_T(string number)   //number为舵机编号
        {
            string sum_str = null;
            string curve = null;
            byte sum = 0;

            byte[] bRx = this.frmain.HexStringToByteArray(number + "03" + "01" + constant.REG_T_BUFFER);
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + number + "03" + "01" + constant.REG_T_BUFFER + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);


            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }

            Delay_ms(10);
        }

        //启动一个主舵机Hermit曲线 number为舵机编号
        public void Move_Curve1(string number)   //启动h曲线
        {
            string num = number;
            string sum_str = null;
            string curve = null;
            byte[] bRx = this.frmain.HexStringToByteArray(num + "04"+ "02" + constant.REG_MODEL + "02");
            byte sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + num + "04" + "02" + constant.REG_MODEL + "02" + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);

            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            Delay_ms(10);
        }

        //启动三个主舵机Hermit曲线
        public void Move_Curve()   //启动h曲线
        {
            string sum_str = null;
            string curve = null;
            byte[] bRx = this.frmain.HexStringToByteArray("FE" + "0D"
               + "02" + "02" + constant.REG_MODEL + "02"
               + "03" + "02" + constant.REG_MODEL + "02"
               + "04" + "02" + constant.REG_MODEL + "02");
            byte sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + "FE" + "0D"
               + "02" + "02" + constant.REG_MODEL + "02"
               + "03" + "02" + constant.REG_MODEL + "02"
               + "04" + "02" + constant.REG_MODEL + "02" + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);

            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            Delay_ms(10);
        }

        //启动四个舵机Hermit曲线
        public void Move_Curve4()   //启动h曲线
        {
            string sum_str = null;
            string curve = null;
            byte[] bRx = this.frmain.HexStringToByteArray("FE" + "11"
                + "01" + "02" + constant.REG_MODEL + "02"
                + "02" + "02" + constant.REG_MODEL + "02"
                + "03" + "02" + constant.REG_MODEL + "02"
                + "04" + "02" + constant.REG_MODEL + "02");
            byte sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + "FE" + "11"
                + "01" + "02" + constant.REG_MODEL + "02"
                + "02" + "02" + constant.REG_MODEL + "02"
                + "03" + "02" + constant.REG_MODEL + "02"
                + "04" + "02" + constant.REG_MODEL + "02" + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);

            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            Delay_ms(10);
        }

        //启动一个主舵机连续曲线  number为舵机编号
        public void Move_T1(string number)    //启动T曲线
        {
            string num = number;
            string sum_str = null;
            string curve = null;
            byte[] bRx = this.frmain.HexStringToByteArray(num + "04" + "02" + constant.REG_MODEL + "01");
            byte sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + num + "04" + "02" + constant.REG_MODEL + "01" + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);

            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            Delay_ms(10);
        }

        //启动三个主舵机连续曲线
        public void Move_T()    //启动T曲线
        {
            string sum_str = null;
            string curve = null;
            byte[] bRx = this.frmain.HexStringToByteArray("FE" + "0D"
               + "02" + "02" + constant.REG_MODEL + "01"
               + "03" + "02" + constant.REG_MODEL + "01"
               + "04" + "02" + constant.REG_MODEL + "01");
            byte sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + "FE" + "0D"
               + "02" + "02" + constant.REG_MODEL + "01"
               + "03" + "02" + constant.REG_MODEL + "01"
               + "04" + "02" + constant.REG_MODEL + "01" + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);

            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            Delay_ms(10);
        }

        //启动四个舵机连续曲线
        public void Move_T4()    //启动T曲线
        {
            string sum_str = null;
            string curve = null;
            byte[] bRx = this.frmain.HexStringToByteArray("FE" + "11"
                + "01" + "02" + constant.REG_MODEL + "01"
                + "02" + "02" + constant.REG_MODEL + "01"
                + "03" + "02" + constant.REG_MODEL + "01"
                + "04" + "02" + constant.REG_MODEL + "01");
            byte sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + "FE" + "11"
                + "01" + "02" + constant.REG_MODEL + "01"
                + "02" + "02" + constant.REG_MODEL + "01"
                + "03" + "02" + constant.REG_MODEL + "01"
                + "04" + "02" + constant.REG_MODEL + "01" + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);

            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            Delay_ms(10);
        }

        //给三个主舵机刹车
        public void Break()   //刹车
        {
            string sum_str = null;
            string curve = null;
            byte sum = 0;

            byte[] bRx = this.frmain.HexStringToByteArray("FE" + "0D"
                + "02" + "02" + constant.BRAKE + "02"
                + "03" + "02" + constant.BRAKE + "02"
                + "04" + "02" + constant.BRAKE + "02");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + "FE" + "0D"
            + "02" + "02" + constant.BRAKE + "02"
            + "03" + "02" + constant.BRAKE + "02"
            + "04" + "02" + constant.BRAKE + "02" + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);


            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
        }

        //给一个主舵机刹车
        public void Break_One(string number)   //刹车  number为舵机编号
        {
            string num = number;

            string sum_str = null;
            string curve = null;
            byte sum = 0;

            byte[] bRx = this.frmain.HexStringToByteArray(num + "04" + "02" + constant.BRAKE + "02");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + num + "04" + "02" + constant.BRAKE + "02" + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);


            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
        }

        //清除某个舵机的缓存区剩余节点数
        public void ClearBuffer(string number, string command)    //清除位置buffer节点   number为舵机号，command为指令，00为位置时间模式，01为连续位置模式
        {
            string sum_str = null;
            string curve = null;
            byte sum = 0;

            byte[] bRx = this.frmain.HexStringToByteArray(number + "03" + "05" + command);
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            curve = "FF" + "FF" + number + "03" + "05" + command + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }

            this.frmain.servosend(curve);


            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            Delay_ms(10);
        }

    }
}
